#include <math.h>
#include <unistd.h>
#include <pthread.h>

#include "lcm/lcm.h"

#include "common/getopt.h"
#include "common/timestamp.h"
#include "lcmtypes/diff_drive_t.h"
#include "lcmtypes/difteleop_t.h"
#include "lcmtypes/simplepose_t.h"
#include "lcmtypes/motor_feedback_t.h"

#include "LinAlg.h"

// will eventually need to extend this to describe the pose of the robot as well
typedef struct Pose_struct {
	// odometry measurements in clicks [left right]
	int32_t lastClicksLR[2];
	int32_t deltaClicksLR[2];

	// timing information
	int64_t lastUtime;
	int64_t deltaUtime;

	//Need to keep track of pose x,y,theta these are world coordinates
	float x;
	float y;
	float theta;

	//Need to keep track of linear and angular velocities - may want a circle buffer of deltas to average over
	float dxdt;
	float dydt;
	float dthetadt;
} Pose;

/* Program for the pandabot to listen in for differential drive commands and execute them directly*/
/* stealing guts out of diff_publisher.c example updates the cmdLR values with the commanded value*/
static void handler_teleop(const lcm_recv_buf_t *rbuf, const char *channel,
                              const difteleop_t *msg, void *user)
{
	//printf("Got teleop\n");
	float *cmdLR = (float *) user;
	// left 
	cmdLR[0] = msg->left;
	// right
	cmdLR[1] = msg->right;
}

static void handler_motor_feedback(const lcm_recv_buf_t *rbuf, const char *channel,
                              const motor_feedback_t *msg, void *user)
{
	Pose *pose = (Pose *) user;

	//printf("Got motor feedback\n");
	//printf("\tLeft: %d \tRight: %d\n", msg->encoders[0], msg->encoders[1]);

	// how much time elapsed since our last encoder reading
	int64_t curtime = timestamp_now();
	pose->deltaUtime = curtime - pose->lastUtime;
	pose->lastUtime = curtime;

	// how much have our encoders changed by
	pose->deltaClicksLR[0] = msg->encoders[0] - pose->lastClicksLR[0]; 
	pose->deltaClicksLR[1] = msg->encoders[1] - pose->lastClicksLR[1];
	pose->lastClicksLR[0] = msg->encoders[0];
	pose->lastClicksLR[1] = msg->encoders[1];

	// generate pose interms of x, y, theta
	float clickperm = 3.021077e-04; 	// meters per click
	float baseline = 0.184;			// baseline distance between the wheels

	float dL = clickperm * pose->deltaClicksLR[0];
	float dR = clickperm * pose->deltaClicksLR[1];

	float d = (dL + dR)/2;			// forwards movement
	float dtheta = (dR - dL)/baseline;  	// change in heading

	float dl[] = {d, 0, 1};
	Vec	*pl = Vec_create(dl,3);
	Matrix *rbtl = Matrix_createRBT2D(0,0,dtheta);						// spits out dx and dy in the local frame
	Matrix *rbt_ltog = Matrix_createRBT2D(pose->x,pose->y,pose->theta); // transforms local to global coordinates

	// this line is a memory leak since we aren't capturing the intermediate vector that is being created!!
	Vec *pg = Matrix_multiplyAV(rbt_ltog,Matrix_multiplyAV(rbtl,pl)); // global x y coordinates

	// generate linear and angular velocities
	float dt = (pose->deltaUtime)*10e-6;			// calculate elapsed time
	pose->dxdt = (pg->val[0] - pose->x)/dt;
	pose->dydt = (pg->val[1] - pose->y)/dt;
	pose->dthetadt = dtheta/dt;

	// store our new pose information
	pose->x = pg->val[0];
	pose->y = pg->val[1];
	pose->theta += dtheta;

	// clean up
	Vec_destroy(pl);
	Vec_destroy(pg);
	Matrix_destroy(rbtl);
	Matrix_destroy(rbt_ltog);
}

//LCM handler threads
void listenerThread(void *arg)
{
	lcm_t *listener = (lcm_t*) arg;
	while(1)
	{
		if (lcm_handle(listener))
		{
		    exit(1);
		}
	}
}

int main(int argc, char *argv[])
{
	// lcm objects
	lcm_t *lcmLocal = lcm_create(NULL); //"udpm://239.255.76.67:7667?ttl=0")
	lcm_t *lcmGlobal = lcm_create("udpm://239.255.76.10:7667?ttl=1");

	// most recent command
	float cmdLR[] = {0.0, 0.0};
	Pose pose = {.lastClicksLR = {0, 0}, .deltaClicksLR = {0, 0}, .lastUtime = timestamp_now(), .deltaUtime = 0};

	// subscribe to the teleoperation messages you don't need the return value
	difteleop_t_subscribe(lcmGlobal, "Team10_TELEOP", handler_teleop, (void*) cmdLR);
	motor_feedback_t_subscribe(lcmLocal, "MOTOR_FEEDBACK", handler_motor_feedback, (void *) &pose);

	//Start listener threads
	pthread_t *listenerLocal;
	pthread_t *listenerGlobal;
	pthread_create(&listenerLocal, NULL, listenerThread, lcmLocal);
	pthread_create(&listenerGlobal, NULL, listenerThread, lcmGlobal);

	usleep(100000); // wait a bit to allow the initial clicks to update

	int cntr = 0;
	int32_t initClicksLR[] = {pose.lastClicksLR[0], pose.lastClicksLR[1]};
	while(1) {
		usleep(100000);
		diff_drive_t cmd = {.left = cmdLR[0], .right = cmdLR[1], .left_enabled = 1, .right_enabled = 1};
		cmd.utime = timestamp_now();

		//printf("Did pose update, %i, %i \n",pose.deltaClicksLR[0],pose.deltaClicksLR[1]);	

		simplepose_t pubPose = {.pos = {pose.x, pose.y}, .vel = {pose.dxdt, pose.dydt}, .orientation = pose.theta, .rotation_rate = pose.dthetadt, .accel = {0, 0}};
		diff_drive_t_publish(lcmLocal,"DIFF_DRIVE", &cmd);
		simplepose_t_publish(lcmGlobal, "Team10_POSE", &pubPose);

		cntr++;
	}
	int32_t finalClicksLR[] ={pose.lastClicksLR[0], pose.lastClicksLR[1]};;

	printf("Final delta, %i, %i \n",finalClicksLR[0]-initClicksLR[0],finalClicksLR[1]-initClicksLR[1]);

	lcm_destroy(lcmLocal);
	lcm_destroy(lcmGlobal);

	return 0;
}
